/*
 *  pack.c
 *  xcode
 *
 *  Created by Emiel Goor,van on 25-01-08.
 *  Copyright 2008 __MyCompanyName__. All rights reserved.
 *
 */

#include "pack.h"
#include "adapt.h"

struct packetQueue_t
{
	struct packetQueue_t *next;
	packet_t *packet;
};

static struct packetQueue_t *pqHead=0;
static struct packetQueue_t *pqTail=0;

// internet checksum, defined in RFC 1071
uint16_t packetChecksum( packet_t* packet, int len )
{
	// total sum
	unsigned long sum = 0;
	
	// pointer to the data
	uint16_t* ptr = (uint16_t*) packet->data;
	
	// get the data
	while( len > 1 )
	{
		// sum the next bit
		sum += *ptr++;
		
		// decrease the length
		len -= sizeof( uint16_t );
	}
	
	// left overs
	if( len > 0 )
		sum += *ptr++;
	
	// 32-bit --> 16-bit
	while( sum >> 16 )
		sum = ( sum & 0xFFFF ) + ( sum >> 16 );
	
	// return the one's complement
	return (uint16_t) ~sum;
}

// same as above, but easier to work with
uint16_t dataChecksum( char* data, int len )
{
	// total sum
	unsigned long sum = 0;
	
	// pointer to the data
	uint16_t* ptr = (uint16_t*) data;
	
	// get the data
	while( len > 1 )
	{
		// sum the next bit
		sum += *ptr++;
		
		// decrease the length
		len -= sizeof( uint16_t );
	}
	
	// left overs
	if( len > 0 )
		sum += *ptr++;
	
	// 32-bit --> 16-bit
	while( sum >> 16 )
		sum = ( sum & 0xFFFF ) + ( sum >> 16 );
	
	// return the one's complement
	return (uint16_t) ~sum;
}

/*
uint16_t packetChecksum( packet_t* packet, int len )
{
	// index
	int i;
	
	// 32-bit sum
	uint32_t sum = 0;
	
	// 16-bit pointer
	uint16_t* ptr = (uint16_t*) packet->data;
	
	// loop through the full length, summing up the 16-bit words
	for( i = 0; i < len; i++ )
		sum += ptr[i];
	
	// take 16 bits and add up the carries
	while( sum >> 16 )
	{
		sum = ( sum & 0xFFFF ) + ( sum >> 16 );
	}
	
	// return the one's complement
	return (uint16_t) ~sum;
}


u16 ip_sum_calc(u16 len_ip_header, u16 *buff) 
{ 
u16 word16; 
u32 sum=0; 
u16 i; 
    
   // make 16 bit words out of every two adjacent 8 bit words in the packet 
   // and add them up 
   for (i=0;i<len_ip_header;i=i+2){ 
      word16 =((buff[i]<<8)&0xFF00)+(buff[i+1]&0xFF); 
      sum = sum + (u32) word16;    
   } 
    
   // take only 16 bits out of the 32 bit sum and add up the carries 
   while (sum>>16) 
     sum = (sum & 0xFFFF)+(sum >> 16); 

   // one's complement the result 
   sum = ~sum; 
    
return ((u16) sum); 
} 

uint16_t packetChecksum(packet_t *packet, int len)
{
	uint32_t sum = 0;
	uint16_t *p = (uint16_t *)packet->data;
	
	while(len > 1)
	{
		sum += *p++;
		if(sum > 0xffff)
			sum = (sum & 0xffff) + 1;
		len -= 2;
		if(len == 1)
		{
			// this only works because we're little endian
			sum += *(uint8_t *)p; 
			if(sum > 0xffff)
				sum = (sum & 0xffff) + 1;
		}
	}
	return (uint16_t)~sum;

}*/

void packetQueueAdd(packet_t *p)
{
	struct packetQueue_t *pq;
		
	pq = (struct packetQueue_t *)kmalloc(sizeof(struct packetQueue_t));
	pq->packet = p;

	if(!pq)
		return;
	
	if(!pqHead)
	{
		pqHead = pqTail = pq;
		pqTail->next = (struct packetQueue_t*) NULL;
		return;
	}
	
	pqTail->next = pq;
	pqTail = pq;
	pqTail->next = (struct packetQueue_t*) NULL;
}

packet_t *packetDequeue(void)
{
	packet_t *p;
	struct packetQueue_t *pq;

	p = pqHead->packet;
	
	if(pqHead->next)
		pqHead = pqHead->next;
	kfree((char*)pq);
	return p;
}

packet_t *packetCreate(uint8_t *data, uint16_t len)
{
	packet_t *pkt = kmalloc(sizeof(packet_t));
	if(!pkt)
		return (packet_t*) NULL;
	
	pkt->data = kmalloc(len);	
	if(!pkt->data)
	{
		kfree((char*)pkt);
		return (packet_t*) NULL;
	}

	pkt->len  = len;
	memcpy(pkt->data, data, len);
	return pkt;
}

packet_t *packetAddHeader(int headersize, packet_t *packet)
{
	int len = packet->len;
	char *data = kmalloc(headersize+packet->len);
	if(!data)
	{
		return (packet_t*) 0;
	}

	memcpy(data+headersize, packet->data, packet->len);
	kfree((char*)packet->data);
	packet->data = data;
	packet->len = headersize + len;
	return packet;
}

packet_t *packetCopy(packet_t *oldPacket)
{
	packet_t *newp = (packet_t *)kmalloc(sizeof(packet_t));
	if(!newp)
		return (packet_t*) NULL;
	
	newp->len = oldPacket->len;
	newp->data = kmalloc(oldPacket->len);
	if(!newp->data)
	{
		kfree((char*)newp);
		return (packet_t*) NULL;
	}
	memcpy(newp->data, oldPacket->data, oldPacket->len);
	return newp;
}